// 发布图像节点源文件

#include "Publisher_node.hpp"

ImgPublisher::ImgPublisher() : Node("image_publisher")
{
  cap_.open("/home/cwb/qd_task/video/armor_red.avi");   //视频路径
  double fps = cap_.get(cv::CAP_PROP_FPS);
  RCLCPP_INFO(this->get_logger(), "发布器已启动，帧率: %.2f FPS", fps);
}

void ImgPublisher::initialize()
{
  double fps = cap_.get(cv::CAP_PROP_FPS);
  it_ = std::make_shared<image_transport::ImageTransport>(shared_from_this());
  publisher_ = it_->advertise("image", 20);
  timer_ = this->create_wall_timer(std::chrono::duration<double, std::milli>(1000/fps), 
  std::bind(&ImgPublisher::pub_image, this));
}

void ImgPublisher::pub_image()
{
    cv::Mat frame, roiImage, proImage, imagePro;
    cap_ >> frame;
    if (frame.empty()) {
        RCLCPP_INFO(this->get_logger(), "再次发布视频");
        //循环播放
        if (cap_.get(cv::CAP_PROP_POS_FRAMES) >= cap_.get(cv::CAP_PROP_FRAME_COUNT)) {
            cap_.set(cv::CAP_PROP_POS_FRAMES, 0);  
        }
        return;
    }
    cv::Rect roiRect(180,350,1080,350);   //创建Rect对象roiRect，截取ROI区域
    roiImage = frame(roiRect);   //利用ROI选取部分区域进行处理,减少其它干扰
    ColorFind_RGB find;
    proImage = find.ColorFind_Red(roiImage);  // 红色分割，得到红色装甲板二值化图像
    opera.imageProcess(proImage, imagePro);   //图像处理
    if (imagePro.channels() != 1 || imagePro.type() != CV_8UC1) {
        cv::cvtColor(imagePro, imagePro, cv::COLOR_BGR2GRAY);
    }
    std::vector<std::vector<cv::Point>> contours;
    std::vector<cv::Vec4i> hierarchy;
    std::vector<LightDescriptor> lightInfos;
    cv::findContours(imagePro, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);  //寻找轮廓
    opera.filterContours(contours, lightInfos);   //筛选轮廓
    double fps = cap_.get(cv::CAP_PROP_FPS);
    opera.armorDetect(lightInfos, roiImage, armors, fps);    //装甲板可视化，读取装甲板位置信息  
    std::stringstream ss;
    ss<< std::fixed << std::setprecision(2) << "Distance: " << globalDistance << "m";
    std::string text = ss.str();
    //输出相机到装甲板中心距离的文本  
    cv::putText(frame, text, {20,70}, cv::FONT_HERSHEY_SIMPLEX, 1.5, cv::Scalar(0, 255, 255), 4);
    roiImage.copyTo(frame(roiRect));

    auto msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frame).toImageMsg();
    msg->header.stamp = this->now();
    msg->header.frame_id = "video_frame";
    publisher_.publish(msg);
 }
 
int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<ImgPublisher>();
  node->initialize();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
